#include "ArbolRitter.h" 
 
 
ArbolRitter::ArbolRitter() 
{ 
 
} 
 
 
double ArbolRitter::calcDist (Vertice a, Vertice b){//pendiente..fijarse como arreglarla con const Vertice &a 
    return sqrt (pow(a.getX() - b.getX(),2) + 
                 pow(a.getY() - b.getY(),2) + 
                 pow(a.getZ() - b.getZ(),2)); 
} 
//haya las coordenadas menores y mayores de una serie de puntos 

ArbolRitter::ArbolRitter(const ArbolRitter &a){ 
    raiz = a.raiz; 
 
} 
 

void ArbolRitter::dividirVector(const int &p, const int &q,vector <Vertice> v, vector <Vertice> &v1, vector <Vertice> &v2){

    unsigned int tamano = v.size();
	unsigned int flag = 0;

    for(unsigned int i = 0; i < tamano; i++){
        double dpi =  calcDist(v[p],v[i]);
        double dqi =   calcDist(v[q],v[i]);
        //cout << "Pos p: " << p << " q: " << q << " i: " << i << " Tamano: " << tamano << endl;
        //cout << "dist p-i: " << dpi  << " dist q-i: " << dqi << endl;
        if(dpi < dqi){
            v1.push_back(v[i]);
        } else if (dpi > dqi){
            v2.push_back(v[i]);
		} else {
			if(flag == 0){
				v1.push_back(v[i]);
				flag = 1;
			} else {
				v2.push_back(v[i]);
				flag = 0;
			}
           // cout << "v1 <- p; v2 <- q" << endl;
        }
    }
    
}


bool ArbolRitter::hayInterseccion(Triangulo t1, Triangulo t2){ 
    double punto1t1[]={t1.getVertice1()->getX(),t1.getVertice1()->getY(),t1.getVertice1()->getZ()}; 
    double punto2t1[]={t1.getVertice2()->getX(),t1.getVertice2()->getY(),t1.getVertice2()->getZ()}; 
    double punto3t1[]={t1.getVertice3()->getX(),t1.getVertice3()->getY(),t1.getVertice3()->getZ()}; 
    double punto1t2[]={t2.getVertice1()->getX(),t2.getVertice1()->getY(),t2.getVertice1()->getZ()}; 
    double punto2t2[]={t2.getVertice2()->getX(),t2.getVertice2()->getY(),t2.getVertice2()->getZ()}; 
    double punto3t2[]={t2.getVertice3()->getX(),t2.getVertice3()->getY(),t2.getVertice3()->getZ()}; 
     
    return NoDivTriTriIsect(punto1t1,punto2t1,punto3t1, punto1t2, punto2t2, punto3t2); 
} 

Vertice ArbolRitter::calcularPromedioPunto(Triangulo t){// calcula una
                                                        // aproximacion al centro_del_triangulot
    Vertice p1 = *(t.getVertice1());
    Vertice p2 = *(t.getVertice2());
    Vertice p3 = *(t.getVertice3());

    return Vertice((p1.getX()+p2.getX()+p3.getX())/3,
                   (p1.getY()+p2.getY()+p3.getY())/3,
                   (p1.getZ()+p2.getZ()+p3.getZ())/3);
}



bool ArbolRitter::hayColision(Triangulo tr){

    Vertice e = calcularPromedioPunto(tr);
    Vertice frVert;
    Nodo cont = *raiz;
    //cout << "p: " << cont.getPrim() << endl;
    
    while(cont.getVector().size() > 1){
        //cout << "cont: " << cont.getVector().size() << endl;
        if(calcDist(e,cont.getVecPosP()) < calcDist(e,cont.getVecPosQ())){
            cont = cont.getNodo(1);
            //cout << "izq" << endl;
        } else {
            cont = cont.getNodo(2);
            //cout << "der" << endl;
        }
    }

    //cout << "cont: " << cont.getVector().size() << endl;
    
    frs = *mapa[cont.getVector()[0]];//triangulo mas cercanco a tr...
    frVert = calcularPromedioPunto(frs);
    if(calcDist(e,frVert) < 10.0){//10.0 es el umbral para determinar si hay
                                 //colision o no...
        return true;
    }else{
        return false;
    }
    //return hayInterseccion(tr, frs);     
}





 
void ArbolRitter::Inicializar(const vector <Triangulo> &t){//Constructor
    cout << "Cargando Arbol de colisiones..." << endl;
    vector <Vertice> bisectrices;
    for(unsigned int i = 0; i < t.size(); i++){      
        Vertice e = calcularPromedioPunto(t[i]);
        bisectrices.push_back(e);
        mapa[e] = &t[i];
    }
    raiz = new Nodo(bisectrices);
    cargarArbol(bisectrices,raiz);
}


void ArbolRitter::cargarArbol(vector <Vertice> &e, Nodo *n){
    if(e.size() > 1){
            vector <Vertice> v1;
            vector <Vertice> v2;
            //Nodo *n = new Nodo(e);
            dividirVector(n->getPrim(), n->getSeg(), e, v1, v2);
            Nodo *n1 = new Nodo(v1);
            Nodo *n2 = new Nodo(v2);
            //cout << "padre: " << n->getVector().size() << endl;
            
           // cout << "v1: " << n1->getVector().size() << endl;
           // cout << "v2: " << n2->getVector().size() << endl;
         
            n->insertarNodo(1,n1);
            n->insertarNodo(2,n2);
            
            cargarArbol(v1,n1);
            cargarArbol(v2,n2);          
    }
    
    
}
   
    


